///*
//        |+X 
//        |
//        |
//+Y------------- -Y
//        |
//        |
//        |-X
//     
//统一坐标系为东北天，即前X左Y



#include "Ano_T265.h"
#include "Drv_usart.h"


Ano_T265 T265;
uint8_t T265_RST[5]={0xAA,0XFF,0X40,0X01,0X01};



u8 _rx_buf[256];
uint8_t j=0;
int16_t raw_x,raw_y=0;
s16 raw_spd_x,raw_spd_y=0;

#define N 10 // 滤波窗口大小

int data_array_x[N]; // 存储窗口内的数据
int sum_x = 0; // 存储窗口内数据的和
int index_x = 0; // 当前数据的索引

// 滑动平均滤波器函数
// 参数：new_data - 新的输入数据
// 返回值：滤波后的数据
int Moving_Average_Filter_x(int new_data) 
{
    sum_x -= data_array_x[index_x]; // 减去窗口中被替换的数据
    data_array_x[index_x] = new_data; // 存储新数据
    sum_x += new_data; // 计算新的和
    index_x = (index_x + 1) % N; // 更新索引
    return sum_x / N; // 返回平均值
}

int data_array_y[N]; // 存储窗口内的数据
int sum_y = 0; // 存储窗口内数据的和
int index_y = 0; // 当前数据的索引

// 滑动平均滤波器函数
// 参数：new_data - 新的输入数据
// 返回值：滤波后的数据
int Moving_Average_Filter_y(int new_data) 
{
    sum_y -= data_array_y[index_y]; // 减去窗口中被替换的数据
    data_array_y[index_y] = new_data; // 存储新数据
    sum_y += new_data; // 计算新的和
    index_y = (index_y + 1) % N; // 更新索引
    return sum_y / N; // 返回平均值
}


void T265_Get_Byte(u8 data)
{
	static u8 _sta = 0;
	static u8 cnt=2;
	
//	_rx_buf[j]=data;
//	j++;
//	if(j>200){j=0;}
	if(_sta==0&&data==0xAA)	//帧头0xAA
	{
		_sta=1;
		_rx_buf[0]=data;
	}
	
	else if(_sta==1&&data==0xFF)	//广播地址，0xFF
	{
		_sta=2;
		_rx_buf[1]=data;
	}
	else if(_sta==2)		//n份数据
	{
		_rx_buf[cnt]=data;
		cnt++;
		if(cnt==10){_sta=3;cnt=2;}
	}
	else if(_sta==3&&data==0xFE)		//帧尾
	{
		raw_x=(int16_t)(_rx_buf[2]<<8)+_rx_buf[3];
		raw_y=(int16_t)(_rx_buf[4]<<8)+_rx_buf[5];
		
		T265.coordinate_x=Moving_Average_Filter_x(raw_x);
		T265.coordinate_y=Moving_Average_Filter_y(raw_y);
		
		
		raw_spd_x=(int16_t)(_rx_buf[6]<<8)+_rx_buf[7];
		raw_spd_y=(int16_t)(_rx_buf[8]<<8)+_rx_buf[9];
		T265.speed_x=-raw_spd_y;
		T265.speed_y=raw_spd_x;
	}
	else
		_sta = 0;
	
}




